#include "SdkMotor.h"

#define DEF_TAG "Motor"

#define DEFULAT_CFG_VALUE (-1)

#define MOTOR0 "/dev/motor0"
#define MOTOR1 "/dev/motor1"

#define DEF_MOTOR_UD_ANGLE_MAX 1000
#define DEF_MOTOR_UD_ANGLE_MIN (-450)
#define DEF_MOTOR_UD_TURN_VAL 120

#define DEF_MOTOR_ANGLE_MAX 9600
#define DEF_MOTOR_ANGLE_MIN (-4900)
#define DEF_MOTOR_TURN_VAL 360

#if CMAKE_ANYKA_EV300L

#define DEF_MOTOR_SPEED 1200  // 频率 单位时间/1秒内的步进总数
#define DEF_MOTOR_STEP 4096   // 一圈的步数总和
#define DEF_MOTOR_TOTAL 3600  // 总运动步数
#define DEF_MOTOR_BOUNDARY 50 // 边界步数

#endif

int g_SdkMotor_Fd[SDK_MOTOR_MAX_NUM];

void Ext_SdkMotor_Init(void)
{
    memset(g_SdkMotor_Fd, 0, sizeof(g_SdkMotor_Fd));

    g_SdkMotor_Fd[SDK_MOTOR_0] = open(MOTOR0, 0);
    if (g_SdkMotor_Fd[SDK_MOTOR_0] < 0)
    {
        perror("fail to open motor device\n");
    }

#if CMAKE_ANYKA_EV300L
    struct motor_parm parm;
    parm.pos = 0; // 当前位置
    parm.speed_step = DEF_MOTOR_SPEED;
    parm.steps_one_circle = DEF_MOTOR_STEP;
    parm.total_steps = DEF_MOTOR_TOTAL;
    parm.boundary_steps = DEF_MOTOR_BOUNDARY;

    if (ioctl(g_SdkMotor_Fd[SDK_MOTOR_0], MOTOR_PARM, &parm))
    {
        perror("MOTOR_CFG fail\n");
    }
#endif

#if CMAKE_SUPPORT_PTZ
    g_SdkMotor_Fd[SDK_MOTOR_1] = open(MOTOR1, 0);
    if (g_SdkMotor_Fd[SDK_MOTOR_1] < 0)
    {
        perror("fail to open motor device\n");
    }
#endif
}

void SdkMotor_Check_Status(int num)
{
    int ret = 0;
    struct motor_message msg;

    do
    {
        usleep(10 * 1000);
        ret = ioctl(g_SdkMotor_Fd[num], MOTOR_GET_STATUS, &msg);
        if (ret)
        {
            PRT_ERR(DEF_TAG, "get status fail\n");
            break;
        }
    } while (msg.status == MOTOR_IS_RUNNING);
}

void Ext_SdkMotor_Wait_Stop(int num)
{
    SdkMotor_Check_Status(num);
}

void Ext_SdkMotor_Up(int num)
{
#if CMAKE_SUPPORT_PTZ
    int distance = DEF_MOTOR_UD_TURN_VAL;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &distance))
    {
        PRT_ERR(DEF_TAG, "turn up fail\n");
    }
#endif
}

void Ext_SdkMotor_Down(int num)
{
#if CMAKE_SUPPORT_PTZ
    int distance = -DEF_MOTOR_UD_TURN_VAL;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &distance))
    {
        PRT_ERR(DEF_TAG, "turn down fail\n");
    }
#endif
}

void Ext_SdkMotor_TurnLeft(int num)
{
    int distance = DEF_MOTOR_TURN_VAL;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &distance))
    {
        PRT_ERR(DEF_TAG, "turn left fail\n");
    }
}

void Ext_SdkMotor_TurnRight(int num)
{
    int distance = (-DEF_MOTOR_TURN_VAL);

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &distance))
    {
        PRT_ERR(DEF_TAG, "turn right fail\n");
    }
}

void Ext_SdkMotor_TurnVal(int num, int val)
{
    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &val))
    {
        PRT_ERR(DEF_TAG, "turn val fail\n");
    }
}

void Ext_SdkMotor_Stop(int num)
{
    int distance = 0;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_STOP, &distance))
    {
        PRT_ERR(DEF_TAG, "stop fail\n");
    }
}

void Ext_SdkMotor_Init_Center(int num)
{
    switch (num)
    {
    case SDK_MOTOR_0:
    {
        int distance = DEF_MOTOR_ANGLE_MAX;

        if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &distance))
        {
            PRT_ERR(DEF_TAG, "motor angle max fail\n");
        }
        SdkMotor_Check_Status(num);

        distance = DEF_MOTOR_ANGLE_MIN;

        if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &distance))
        {
            PRT_ERR(DEF_TAG, "motor angle min fail\n");
        }
        SdkMotor_Check_Status(num);
    }
    break;

    case SDK_MOTOR_1:
    {
#if CMAKE_SUPPORT_PTZ
        int distance = DEF_MOTOR_UD_ANGLE_MAX;

        if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &distance))
        {
            PRT_ERR(DEF_TAG, "motor angle max fail\n");
        }

        SdkMotor_Check_Status(num);
        distance = DEF_MOTOR_UD_ANGLE_MIN;
        if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE_NOLIMIT, &distance))
        {
            PRT_ERR(DEF_TAG, "motor angle min fail\n");
        }
        SdkMotor_Check_Status(num);
#endif
    }
    break;

    default:
        break;
    }
}

void Ext_SdkMotor_Destroy(void)
{
    int i = 0;
    for (i = 0; i < SDK_MOTOR_MAX_NUM; i++)
    {
        if (g_SdkMotor_Fd[i] > 0)
        {
            close(g_SdkMotor_Fd[i]);
        }
    }
}
